kmlwriteline('traj.kml', output_pos(:,1),output_pos(:,2), 'Color','r', 'Width',2);